/*
 * @Author: LVGRAPE
 * @Date: 2023-09-08 17:56:24
 * @LastEditTime: 2024-07-12 16:06:09
 * @LastEditors: LVGRAPE
 * @Description:
 * @FilePath: \zino-fc-v4\ZINO\rc\rc.c
 * 要啥没啥，爱咋咋的
 */
#if defined(ZINO_RC_HOPPING)
#include "drv_common.h"
#include <nrf24l01.h>
#include <rtconfig.h>
#include <rtthread.h>
#include "rc.h"

#include "zino.h"
#include "maths.h"
// #include "fc_error.h"
#define DBG_TAG "hopping"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define CRC_MAGICBEE 0XED
#define SI24R1_SPI_DEV_NAME "spi_nrf"
#define SI24R1_CE_PIN GET_PIN(C, 4)
#define SI24R1_IRQ_PIN GET_PIN(C, 5)

#define RC_HOPPING_ENABLE 1

ZINO_CONFIG_REGISTER(rc_pair_data_t, pair_data, ZINO_CONFIG_IDX_RC, 1);

ZINO_COMMON_RC_CHANNEL_s zinoCommonRcData;
// uint32_t fc_error_flag = 0;
enum wl_msg_head
{
    WL_MSG_HEAD_NULL = 0,
    WL_MSG_HEAD_RC,          // rc 通道数据
    WL_MSG_HEAD_PAIR_REQ,    // 对频请求
    WL_MSG_HEAD_PAIR_COMFIR, // 对频确认
    WL_MSG_HEAD_FC_STATUS,   // 飞控状态

};
// const static char* ROLE_TABLE[] = { "PTX", "PRX" };
struct rc_handle rchandle =
    {
        .pair_addr = "UAVI1",
        .pair_channel = 33, // 对频用频道
        .pair_fc_handshake_byte = 0xfc,
        .pair_rc_handshake_byte = 0x9c,
        .pair_handshake_byte = 0x0A,
        .pair_handshake_byte_v1 = 0x1A,
        .pair_sync_time = 0,
        .pair_wait_time = 30, // 对频最大等待时间，超时退出
        .pairConfirmTime = 0,
        .address = {1, 2, 3, 4, 5},
        .rf_channel = 33,
        .min_channel = 0,  // 跳频时最低频道(2.4G ISM: 2400~2483.5MHz)
        .max_channel = 83, // 跳频时最高频道(2.4G ISM: 2400~2483.5MHz)
        .hoppingSetting = RC_HOPPING_SET_NONE,

};
/**
 * @brief 接收完成处理函数
 *
 * @param nrf24
 * @param data
 * @param len
 * @param pipe
 */
static void rx_ind(struct si24r1 *s, uint8_t *data, uint8_t len, int pipe)
{
    /*! Don't need to care the pipe if the role is ROLE_PTX */
    // rt_kprintf("(p%d)_%d: ", pipe, len);
    static uint8_t ackCnt = 0;
    uint8_t crc = crc8_dvb_s2_update(CRC_MAGICBEE, data, len - 1);
    for (int i = 0; i < len; i++)
    {
        rt_kprintf("%02X ", rchandle.rx_msg.raw[i]);
    }
    rt_kprintf("\n");
    if (crc == data[len - 1])
    {
        LOG_D("Crc ok!");
        rchandle.rxCnt++;
        // rt_memcpy(rchandle.rx_msg.raw, data, len);
        rt_memcpy(zinoCommonRcData.raw, data, sizeof(zinoCommonRcData.raw));

        rchandle.rx_msg.v.channel[0] = zinoCommonRcData.values.stick.LLR;

        // rt_kprintf("rchandle.rx_msg.raw:(%d) ", rchandle.mode);

        // LOG_D("RC:h%02x i%02x %d \t %d \t %d \t %d \t %d \t %X \t %X \t %d \t %dHz",
        //     rchandle.rx_msg.v.head,
        //     rchandle.rx_msg.v.index,
        //     (int16_t)rchandle.rx_msg.v.channel[0],
        //     (int16_t)rchandle.rx_msg.v.channel[1],
        //     (int16_t)rchandle.rx_msg.v.channel[2],
        //     (int16_t)rchandle.rx_msg.v.channel[3],
        //     (int16_t)rchandle.rx_msg.v.channel[4],
        //     (int16_t)rchandle.rx_msg.v.channel[5],
        //     (int16_t)rchandle.rx_msg.v.channel[6],
        //     (int16_t)rchandle.rx_msg.v.channel[7],
        //     rchandle.rx_rssi
        // );
        if (rchandle.mode == RC_MODE_PAIRING) // return pairing data
        {
            if (rchandle.rx_msg.v.head == WL_MSG_HEAD_PAIR_REQ) // 回复新地址
            {
                rchandle.newAddress[0] = rchandle.rx_msg.raw[2];
                rchandle.newAddress[1] = rchandle.rx_msg.raw[3];
                rchandle.newAddress[2] = rchandle.rx_msg.raw[4];
                rchandle.newAddress[3] = rchandle.rx_msg.raw[5];
                rchandle.newAddress[4] = rchandle.rx_msg.raw[6];
                rchandle.newRfChannel = rchandle.rx_msg.raw[7];

                rt_memset(rchandle.tx_msg.raw, 0, sizeof(rchandle.tx_msg.raw));
                rchandle.tx_msg.v.head = WL_MSG_HEAD_PAIR_REQ;
                rchandle.tx_msg.v.index = ackCnt;
                rchandle.tx_msg.raw[2] = rchandle.newAddress[0];
                rchandle.tx_msg.raw[3] = rchandle.newAddress[1];
                rchandle.tx_msg.raw[4] = rchandle.newAddress[2];
                rchandle.tx_msg.raw[5] = rchandle.newAddress[3];
                rchandle.tx_msg.raw[6] = rchandle.newAddress[4];
                rchandle.tx_msg.raw[7] = rchandle.newRfChannel;

                rchandle.pairConfirmTime = 0;
                ackCnt = 0;
            }
            else if (rchandle.rx_msg.v.head == WL_MSG_HEAD_PAIR_COMFIR) // 对方确认
            {
                rchandle.tx_msg.v.head = WL_MSG_HEAD_PAIR_COMFIR;
                rchandle.tx_msg.v.index = ackCnt;
                LOG_D("WL_MSG_HEAD_PAIR_COMFIR:%d\n", rchandle.rx_msg.v.index);
                if (rchandle.rx_msg.v.index > 2)
                {
                    rchandle.pairConfirmTime = millis();
                }
            }
            rt_memset(rchandle.rx_msg.v.channel, 0, sizeof(rchandle.rx_msg.v.channel));
        } // end of pair
        else // 通常情况回复飞控状态
        {
            // FC_ERROR_SET(FC_STANDBY);
            // rchandle.tx_msg.v.head = WL_MSG_HEAD_FC_STATUS;
            // rchandle.tx_msg.v.index = ackCnt;
            // rchandle.tx_msg.raw[2] = (fc_error_flag >> 24) & 0xff;
            // rchandle.tx_msg.raw[3] = (fc_error_flag >> 16) & 0xff;
            // rchandle.tx_msg.raw[4] = (fc_error_flag >> 8) & 0xff;
            // rchandle.tx_msg.raw[5] = (fc_error_flag) & 0xff;
        }
    } // end of crc ok
    rchandle.tx_msg.v.crc = crc8_dvb_s2_update(CRC_MAGICBEE, rchandle.tx_msg.raw, RTX_BUFFER_SIZE - 1);
    si24r1_send_data(s, rchandle.tx_msg.raw, 8, 0); // 8 bytes ack enough, or would lost signal
    ackCnt++;

    // LOG_D("RC:%dHz \n",
    // rchandle.rx_rssi
    // );
}

/**
 * @brief 发送完成处理函数
 *
 * @param nrf24
 * @param pipe
 */
static void tx_done(struct si24r1 *s, int pipe)
{
}

static struct si24r1_callback si24r1_cb = {
    .rx_ind = rx_ind,
    .tx_done = tx_done,
};
/**
 * @brief 摇控器si24r1的配置，
 * 信号丢失时，参数变化时，需要调用一下来更新si24r1的寄存器配置
 *
 * @param rc
 */
void rc_si24r1_setting(struct rc_handle *rc)
{

    // #if 0
    //     rt_memcpy(rc->address, "test3", 5);
    //     rc->rf_channel = 125;
    // #else
    //     rt_memcpy(rc->address, "test4", 5);
    //     rc->rf_channel = 130;
    // #endif
    rt_memset(&rc->si24r1.onchip_config, 0, sizeof(rc->si24r1.onchip_config));
    if (rc->mode == RC_MODE_PAIRING)
    {
        rt_memcpy(rc->si24r1.onchip_config.tx_addr.addr, rc->pair_addr, 5);
        rt_memcpy(rc->si24r1.onchip_config.rx_addr_p0.addr, rc->pair_addr, 5);
        rc->si24r1.onchip_config.rf_ch.RF_CH = rc->pair_channel;
    }
    else
    {
        rt_memcpy(rc->si24r1.onchip_config.tx_addr.addr, rc->address, 5);
        rt_memcpy(rc->si24r1.onchip_config.rx_addr_p0.addr, rc->address, 5);
        rc->si24r1.onchip_config.rf_ch.RF_CH = rc->rf_channel;
    }

    rc->si24r1.onchip_config.en_rxaddr.ERX_P0 = 1;
    rc->si24r1.onchip_config.rx_pw_p0.RX_PW_P0 = RTX_BUFFER_SIZE;
    rc->si24r1.onchip_config.setup_aw.AW = 0x03;
    rc->si24r1.onchip_config.feature.EN_DPL = 1;
    rc->si24r1.onchip_config.feature.EN_ACK_PAYd = 1;
    rc->si24r1.onchip_config.rf_setup.RF_DR_HIGH = 1;
    rc->si24r1.onchip_config.rf_setup.RF_PWR = 6;
    rc->si24r1.onchip_config.config.EN_CRC = 1;
    rc->si24r1.onchip_config.config.CRCO = 1;
    rc->si24r1.onchip_config.config.PWR_UP = 1;
    rc->si24r1.onchip_config.config.PRIM_RX = 1;
    rc->si24r1.onchip_config.dynpd.DPL_P0 = 1;

    rc->si24r1.onchip_config.status.MAX_RT = 1; // 清除中断
    rc->si24r1.onchip_config.status.RX_DR = 1;  // 清除中断
    rc->si24r1.onchip_config.status.TX_DS = 1;  // 清除中断
    rc->si24r1.onchip_config.en_aa.ENAA_P0 = 1;
    rc->si24r1.onchip_config.setup_retr.ARC = 10; // 10次
    rc->si24r1.onchip_config.setup_retr.ARD = 1;  // 250us

    rc->si24r1.user_config.role = SI24R1_ROLE_RX;

    si24r1_update_onchip_config(&rc->si24r1);
    // if (rc->mode == RC_MODE_PAIRING)
    //     si24r1_check_all_reg(&rc->si24r1);
}

void si24r1_thread(void *param)
{
    rt_err_t ret = RT_EOK;
    struct si24r1 *si = (struct si24r1 *)param;
    LOG_I("2.4G wireless handle:%s\n", PKG_NRF24L01_VERSION);
    do
    {
        ret = si24r1_init(si, SI24R1_SPI_DEV_NAME, SI24R1_CE_PIN, SI24R1_IRQ_PIN, &si24r1_cb);
        if (ret != RT_EOK)
        {
            LOG_E("Failed to init si24r1. Next try in 3 second.");
            rt_thread_mdelay(3000);
        }
    } while (ret != RT_EOK);
    LOG_I("si24r1_thread running...");
    rchandle.si24r1.flags.using_irq = 1;
    rc_si24r1_setting(&rchandle);
    si24r1_check_all_reg(&rchandle.si24r1);
    rt_sem_release(rchandle.si24r1.sem);
    while (1)
    {
        si24r1_run(si);
        if (!si->flags.using_irq)
        {
            rt_thread_mdelay(14);
        }
    }
    // return 0;
}
void rc_into_pair_mode()
{
    if (rchandle.mode != RC_MODE_PAIRING)
    {
        rchandle.mode = RC_MODE_PAIRING;
        // FC_ERROR_SET(FC_RC_PAIRING);
        // FC_ERROR_RESET(FC_RC_LOST);
        rchandle.pair_sync_time = 0;
        rchandle.pairConfirmTime = 0;

        // rc_si24r1_setting(&rchandle);
        LOG_D("pair mode \n\r");
    }
    else
    {
        LOG_D("already pair mode \n\r");
    }
}
void rc_into_normal_mode()
{
    if (rchandle.mode != RC_MODE_NORMAL)
    {
        // FC_ERROR_RESET(FC_RC_PAIRING);
        rchandle.mode = RC_MODE_NORMAL;
        LOG_D("normal mode \n\r");
        // rc_si24r1_setting(&rchandle);
    }
    else
    {
        LOG_D("already normal mode \n\r");
    }
}
void rc_keyActionHandle(uint8_t times)
{
    LOG_D("keyActionHandle times:%d %d %d\n", times, rchandle.mode, rchandle.pair_sync_time);
    if (times == 2)
    {
        if (rchandle.pair_sync_time > 10)
        {
            rchandle.pair_sync_time = 0;
            rc_into_normal_mode();
            // rc_si24r1_setting(rc);
        }
        else
        {
            rc_into_pair_mode();
        }
    }
}
static void rc_second_timer_cb(void *parameter)
{
    struct rc_handle *rc = (struct rc_handle *)parameter;
    static uint8_t lostCnt = 0;
    rc->rx_rssi = rc->rxCnt;
    rc->rxCnt = 0;
    LOG_D("RC:%dHz %d\n",
          rchandle.rx_rssi,
          rc->pairConfirmTime);
    if (rc->mode == RC_MODE_PAIRING)
    {
        rc->pair_sync_time++;
        if (rc->pair_sync_time >= rc->pair_wait_time)
        {
            LOG_E("pair fail!\n");
            rc->pair_sync_time = 0;
            rc->pairConfirmTime = 0;
            rc_into_normal_mode();
            // rt_event_send(rc->event, RC_EVENT_PAIRING_FAIL);
        }
    }
    if (rc->rx_rssi == 0)
    {
        if (lostCnt++ >= 3)
        {
            lostCnt = 0;
            // rt_event_send(rc->event, RC_EVENT_LOST);
        }
        if ((rc->mode == RC_MODE_PAIRING) && (rc->pairConfirmTime != 0) && millis() > (1000 + rc->pairConfirmTime))
        {
            // NOTE 对频模式，收到确认，然后没信号了，说明遥控器也收到确认了，然后跳到新频道上了。
            rc->mode = RC_MODE_NORMAL;
            rc->state = RC_PAIR_SUCCESS;
            // rt_event_send(rc->event, RC_EVENT_PAIRING_SUCCESS);
        }
    }
}
void rx_handle(struct rc_handle *rc)
{
    uint8_t crc = crc8_dvb_s2_update(CRC_MAGICBEE, rc->rx_msg.raw, rc->rx_len - 1);
    if (crc == rc->rx_msg.raw[rc->rx_len - 1])
    {
        // LOG_D("CRC OK\n");
        rc->rxCnt++;
        if (rc->mode == RC_MODE_PAIRING) // return pairing data
        {
            if (rc->rx_msg.v.head == WL_MSG_HEAD_PAIR_REQ) // 回复新地址
            {
                rc->newAddress[0] = rc->rx_msg.raw[2];
                rc->newAddress[1] = rc->rx_msg.raw[3];
                rc->newAddress[2] = rc->rx_msg.raw[4];
                rc->newAddress[3] = rc->rx_msg.raw[5];
                rc->newAddress[4] = rc->rx_msg.raw[6];
                rc->newRfChannel = rc->rx_msg.raw[7];

                rt_memset(rc->tx_msg.raw, 0, sizeof(rc->tx_msg.raw));
                rc->tx_msg.v.head = WL_MSG_HEAD_PAIR_REQ;
                rc->tx_msg.v.index = rc->rxCnt;
                rc->tx_msg.raw[2] = rc->newAddress[0];
                rc->tx_msg.raw[3] = rc->newAddress[1];
                rc->tx_msg.raw[4] = rc->newAddress[2];
                rc->tx_msg.raw[5] = rc->newAddress[3];
                rc->tx_msg.raw[6] = rc->newAddress[4];
                rc->tx_msg.raw[7] = rc->newRfChannel;

                rc->pairConfirmTime = 0;
                // ackCnt = 0;
            }
            else if (rc->rx_msg.v.head == WL_MSG_HEAD_PAIR_COMFIR) // 对方确认
            {
                rc->tx_msg.v.head = WL_MSG_HEAD_PAIR_COMFIR;
                rc->tx_msg.v.index = rc->rxCnt;
                LOG_D("WL_MSG_HEAD_PAIR_COMFIR:%d\n", rc->rx_msg.v.index);
                if (rc->rx_msg.v.index > 2)
                {
                    rc->pairConfirmTime = millis();
                }
            }
            rt_memset(rc->rx_msg.v.channel, 0, sizeof(rc->rx_msg.v.channel));
        } // end of pair
        else // 通常情况回复飞控状态
        {
            // for(int i = 0; i < 20; i++)
            // {
            //     rt_kprintf("%02x\t",rc->rx_msg.raw[i]);
            // }
            // rt_kprintf("%d\n");
            // FC_ERROR_SET(FC_STANDBY);
            rt_memcpy(&zinoCommonRcData.raw, rc->rx_msg.raw, sizeof(zinoCommonRcData.raw));
            // for(int i = 0; i < 18; i++)
            // {
            //     rt_kprintf("%02X ", zinoCommonRcData.raw[i]);
            // }
            // rt_kprintf("\n");
            // rt_kprintf("stick: %04d %04d %04d %04d\n",
            // zinoCommonRcData.values.stick.RLR,
            // zinoCommonRcData.values.stick.RUD,
            // zinoCommonRcData.values.stick.LLR,
            // zinoCommonRcData.values.stick.LUD);
            // rc->tx_msg.v.head = WL_MSG_HEAD_FC_STATUS;
            // rc->tx_msg.v.index = rc->rxCnt;
            // rc->tx_msg.raw[2] = (fc_error_flag >> 24) & 0xff;
            // rc->tx_msg.raw[3] = (fc_error_flag >> 16) & 0xff;
            // rc->tx_msg.raw[4] = (fc_error_flag >> 8) & 0xff;
            // rc->tx_msg.raw[5] = (fc_error_flag) & 0xff;
        }
    } // end of crc ok
}

/*NOTE RC hopping logic
TX: |CH1---14ms|CH2---14ms|CH3---14ms|CH4---14ms|CH5---14ms|
RX: |CHx--------------get rx jump next----------or wait70ms|

transmitter will send a message every 14ms on the same channel than jump to next channel. If no ack, it sends 14 times every 1ms.
receiver will be listening on the same channel for 70ms (14ms*5=70ms), than jump to next channel. If get message, it will jump to next channel immediately.
*/
static void rc_thread(void *param)
{
    rt_err_t ret = RT_EOK;
    struct rc_handle *rc = (struct rc_handle *)param;
    // LOG_I("2.4G wireless handle:%s\n", PKG_SI24R1_VERSION);
    do
    {
        ret = si24r1_init(&rc->si24r1, SI24R1_SPI_DEV_NAME, SI24R1_CE_PIN, SI24R1_IRQ_PIN, &si24r1_cb);
        if (ret != RT_EOK)
        {
            LOG_E("Failed to init si24r1. Next try in 3 second.");
            rt_thread_mdelay(3000);
        }
    } while (ret != RT_EOK);
    LOG_I("si24r1_thread running...");
    rc->si24r1.flags.using_irq = 0;

    rc->second_timer = rt_timer_create("rc_1s", rc_second_timer_cb, rc, 1000, RT_TIMER_FLAG_PERIODIC);
    rt_timer_start(rc->second_timer);

    rc_si24r1_setting(rc);
    si24r1_check_all_reg(&rc->si24r1);
    rc->state = RC_RECEIVING;
    LOG_D("rc hopping...");
    while (1)
    {
        rt_sem_take(rc->si24r1.sem, 70); // 14ms * 5_jump=70ms
        if (rc->mode != rc->last_mode)
        {
            rc->last_mode = rc->mode;
            if (rc->state == RC_PAIR_SUCCESS)
            {
                LOG_D("PAIR_SUCCESS");
                rc->state = RC_RECEIVING;
                rc->address[0] = rc->newAddress[0];
                rc->address[1] = rc->newAddress[1];
                rc->address[2] = rc->newAddress[2];
                rc->address[3] = rc->newAddress[3];
                rc->address[4] = rc->newAddress[4];
                rc->hoppingSetting = RC_HOPPING_SET_NONE;
                rc->hopping[0] = (rc->address[0] / 2 + rc->min_channel) % rc->max_channel;
                rc->hopping[1] = (rc->address[1] / 2 + rc->min_channel) % rc->max_channel;
                rc->hopping[2] = (rc->address[2] / 2 + rc->min_channel) % rc->max_channel;
                rc->hopping[3] = (rc->address[3] / 2 + rc->min_channel) % rc->max_channel;
                rc->hopping[4] = (rc->address[4] / 2 + rc->min_channel) % rc->max_channel;
                rc->hopping_cnt = 0;
                rc->rf_channel = rc->hopping[rc->hopping_cnt];
                pair_data()->flag = 0xdd;
                pair_data()->address[0] = rc->address[0];
                pair_data()->address[1] = rc->address[1];
                pair_data()->address[2] = rc->address[2];
                pair_data()->address[3] = rc->address[3];
                pair_data()->address[4] = rc->address[4];
                pair_data()->rf_channel = rc->rf_channel;
                pair_data()->hoppingSetting = rc->hoppingSetting;
                zino_config_save_all();
            }
            rc_si24r1_setting(rc);
        }

        if (rt_pin_read(rc->si24r1.port.irq_pin) == PIN_LOW)
        {
            rc->si24r1.port.ops->read_reg(&rc->si24r1.port, SI24R1_REG_STATUS, (uint8_t *)&rc->si24r1.onchip_config.status, 1);
            rc->si24r1.port.ops->write_reg(&rc->si24r1.port, SI24R1_REG_STATUS, (uint8_t *)&rc->si24r1.onchip_config.status, 1);
            if (rc->si24r1.onchip_config.status.RX_DR)
            {
                // LOG_D("RX_DR:%d", rc->rf_channel);
                rc->si24r1.port.ops->read(&rc->si24r1.port, SI24R1_CMD_R_RX_PL_WID, &rc->rx_len, 1);
                rc->si24r1.port.ops->read(&rc->si24r1.port, SI24R1_CMD_R_RX_PAYLOAD, rc->rx_msg.raw, rc->rx_len);
                rc->si24r1.port.ops->write(&rc->si24r1.port, SI24R1_CMD_FLUSH_RX, NULL, 0);
                rx_handle(rc);
                rchandle.tx_msg.v.crc = crc8_dvb_s2_update(CRC_MAGICBEE, rchandle.tx_msg.raw, RTX_BUFFER_SIZE - 1);
                rc->si24r1.port.ops->write(&rc->si24r1.port, SI24R1_CMD_W_ACK_PAYLOAD, rchandle.tx_msg.raw, 8); // 8 bytes ack enough, or would lost signal
            }
            if (rc->si24r1.onchip_config.status.TX_DS)
            {
                // LOG_D("TX_DS:%d", rc->rf_channel);
            }
            if (rc->si24r1.onchip_config.status.MAX_RT)
            {
                // LOG_D("MAX_RT:%d", rc->rf_channel);
            }
            if (rc->mode != RC_MODE_PAIRING)
            {
                if (rc->hoppingSetting == RC_HOPPING_SET_NONE)
                {
                    rc->hopping_cnt++;
                    if (rc->hopping_cnt >= sizeof(rc->hopping))
                    {
                        rc->hopping_cnt = 0;
                    }
                    rc->rf_channel = rc->hopping[rc->hopping_cnt];
                }
                else
                {
                    rc->rf_channel = rc->hoppingSetting & 0x7f;
                }
                rc->si24r1.onchip_config.rf_ch.RF_CH = rc->rf_channel;
                rc->si24r1.port.ops->write_reg(&rc->si24r1.port, SI24R1_REG_RF_CH, (uint8_t *)&rc->si24r1.onchip_config.rf_ch, 1);

                // LOG_D("hopping to %d", rc->rf_channel);
            }
        }
        else
        {
            rc_si24r1_setting(rc);
        }
    }
}
static int rc_task_init(void)
{
    struct rc_handle *rc = &rchandle;
    rt_thread_t thread2;
    if (pair_data()->flag == 0xdd)
    {
        rchandle.address[0] = pair_data()->address[0];
        rchandle.address[1] = pair_data()->address[1];
        rchandle.address[2] = pair_data()->address[2];
        rchandle.address[3] = pair_data()->address[3];
        rchandle.address[4] = pair_data()->address[4];
        rchandle.rf_channel = pair_data()->rf_channel;
        rchandle.hoppingSetting = pair_data()->hoppingSetting;
        LOG_D("Loading save data:\n");
        LOG_D("address[0] = %x\n", rchandle.address[0]);
        LOG_D("address[1] = %x\n", rchandle.address[1]);
        LOG_D("address[2] = %x\n", rchandle.address[2]);
        LOG_D("address[3] = %x\n", rchandle.address[3]);
        LOG_D("address[4] = %x\n", rchandle.address[4]);
        LOG_D("rf_channel = %x\n", rchandle.rf_channel);
        LOG_D("hoppingSetting = %x\n", rchandle.hoppingSetting);
    }
    else
    {
        rchandle.hoppingSetting = RC_HOPPING_SET_NONE;
    }
#if RC_HOPPING_ENABLE
    rchandle.hopping[0] = (rchandle.address[0] / 2 + rchandle.min_channel) % rchandle.max_channel;
    rchandle.hopping[1] = (rchandle.address[1] / 2 + rchandle.min_channel) % rchandle.max_channel;
    rchandle.hopping[2] = (rchandle.address[2] / 2 + rchandle.min_channel) % rchandle.max_channel;
    rchandle.hopping[3] = (rchandle.address[3] / 2 + rchandle.min_channel) % rchandle.max_channel;
    rchandle.hopping[4] = (rchandle.address[4] / 2 + rchandle.min_channel) % rchandle.max_channel;
    rchandle.hopping_cnt = 0;
    rchandle.rf_channel = rchandle.hopping[rchandle.hopping_cnt];
    LOG_D("Hopping:%d-%d-%d-%d-%d\n", rchandle.hopping[0], rchandle.hopping[1], rchandle.hopping[2], rchandle.hopping[3], rchandle.hopping[4]);
#endif
    rchandle.mode = RC_MODE_NORMAL;
    thread2 = rt_thread_create("rc task", rc_thread, rc, 512, RT_THREAD_PRIORITY_MAX / 2, 20);
    if (thread2 != RT_NULL)
        rt_thread_startup(thread2);
    else
        LOG_E("rc task creat fail!\n");

    return RT_EOK;
}
void rc_set(int arg_c, char **argv)
{
    if (rt_strcmp(argv[1], "pair") == 0)
    {
        rc_into_pair_mode();
    }
    else if (rt_strcmp(argv[1], "hopping") == 0)
    {
        rchandle.hoppingSetting = atoi(argv[2]);
        LOG_D("hoppingSetting = %d\n", rchandle.hoppingSetting);
    }
    else if (rt_strcmp(argv[1], "addr") == 0)
    {
        if (arg_c >= 7)
        {
            for (int i = 0; i < 5; i++)
            {
                rchandle.address[i] = atoi(argv[i + 2]);
                LOG_D("address[%d] = %d\n", i, rchandle.address[i]);
            }
        }
        else
        {
            rt_kprintf("Usage: rc_set addr <addr1>,<addr2>,<addr3>,<addr4>,<addr5>\n");
        }
    }
    else if (rt_strcmp(argv[1], "save") == 0)
    {
        LOG_D("save rc config\n");
        pair_data()->flag = 0xdd;
        pair_data()->address[0] = rchandle.address[0];
        pair_data()->address[1] = rchandle.address[1];
        pair_data()->address[2] = rchandle.address[2];
        pair_data()->address[3] = rchandle.address[3];
        pair_data()->address[4] = rchandle.address[4];
        pair_data()->rf_channel = rchandle.rf_channel;
        pair_data()->hoppingSetting = rchandle.hoppingSetting;
        zino_config_save_all();
    }
    else
    {
        rt_kprintf("Usage: rc_set hopping <0~127>, 255 is auto hopping\n");
        rt_kprintf("Usage: rc_set addr <addr1>,<addr2>,<addr3>,<addr4>,<addr5>\n");
        rt_kprintf("Usage: rc_set save\n");
    }
}

uint16_t get_rc_channel(uint8_t rcchannel)
{
    if (rcchannel < 8)
        return rchandle.rx_msg.v.channel[rcchannel];
    return 0;
}

struct rc_handle *get_rc_channel_ram(void)
{
    return &rchandle;
}
uint16_t get_rc_fps()
{
    return rchandle.mode == RC_MODE_NORMAL ? rchandle.rx_rssi : 0;
}

MSH_CMD_EXPORT(rc_set, set rc);
ZINO_APP_EXPORT(rc_task_init);

#endif
